import pyzed.sl as sl
import time
import math
import cv2


if __name__ == "__main__":
    print('start')
    zed = sl.Camera()
    init_params = sl.InitParameters()
    init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_ULTRA
    init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720

    err = zed.open(init_params)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    runtime_parameters = sl.RuntimeParameters()
    runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD

    image = sl.Mat()

    key = ''
    cv2.namedWindow("ZED")
    # cv2.namedWindow("ZED", cv2.WINDOW_GUI_NORMAL)
    # cv2.setWindowProperty('ZED', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)

    while key != 27:
        err = zed.grab(runtime_parameters)
        if err == sl.ERROR_CODE.SUCCESS:
            try:
                zed.retrieve_image(image, sl.VIEW.VIEW_LEFT)
                img = image.get_data()
                cv2.imshow("ZED", img)

            except IndexError:
                pass

        key = cv2.waitKey(15)

    cv2.destroyAllWindows()
    zed.close()
